#include "step_motor_user.h"
#include "ws_constant.h"
#include "elog.h"
#include "elog_file.h"

#include <unistd.h>
#include <cmath>

int StepMotorUser::resetAndRunInDegreeInParallel(int reset_dir, double angle_from_reference, int direction_from_reference, double reference_position) {
    if (!future_.valid()) {
        future_ = std::async(std::launch::async, &StepMotorUser::resetAndRunInDegree, this, reset_dir, 
                            angle_from_reference, direction_from_reference, reference_position);
        return 0;
    } else {
        // log_e("INVALID COMMAND: StepMotor %d is reseting in parallel", motor_id_);
        return -CODE_STEP_MOTOR_PARALLEL_IN;
    }
}

int StepMotorUser::resetAndRunInPulseInParallel(int reset_dir, int pulse_from_reference, int direction_from_reference, int reference_position) {
    if (!future_.valid()) {
        future_ = std::async(std::launch::async, &StepMotorUser::resetAndRunInPulse, this, reset_dir, 
                            pulse_from_reference, direction_from_reference, reference_position);
        return 0;
    } else {
        // log_e("INVALID COMMAND: StepMotor %d is reseting in parallel", motor_id_);
        return -CODE_STEP_MOTOR_PARALLEL_IN;
    }
}

int StepMotorUser::parallelReturn() {
    if (future_.valid()) {
        return future_.get();
    } else {
        // log_e("INVALID COMMAND: StepMotor %d has ended parallel", motor_id_);
        return -CODE_STEP_MOTOR_PARALLEL_END;
    }
}

int StepMotorUser::resetAndRunInDegree(int reset_dir, double angle_from_reference, int direction_from_reference, double reference_position) {
    // 复位，回弹，到指定位置，都使用4细分
    const int resolution_level = STEP_MOTOR_MINISTEP_4;
    const int resolution = 4;

    double start_position = 0;
    int ret = resetInDegree(reset_dir, start_position);
    if (ret < 0)
        return ret;

    if(reference_position == STEP_MOTOR_START_RESET_POSITION)
        reference_position = start_position;

    double end_position = 0;
    ret = getEndPositionInDegree(angle_from_reference, resolution_level, direction_from_reference, end_position, reference_position);
    if (ret < 0)
        return ret;

    ret = setParameterInPositionInDegree(end_position, max_frequency_, resolution_level, STEP_MOTOR_RAMP_PULSE_0, STEP_MOTOR_HIRES_MODE_OFF);
    if (ret < 0)
        return ret;

    run();

    waitMotorToStop(); 

    return ret;
}

int StepMotorUser::resetAndRunInPulse(int reset_dir, int pulse_from_reference, int direction_from_reference, int reference_position) {
    // 复位，回弹，到指定位置，都使用4细分
    const int resolution_level = STEP_MOTOR_MINISTEP_4;
    const int resolution = 4;

    int start_position = 0;
    int ret = resetInPulse(reset_dir, start_position);
    if (ret < 0)
        return ret;

    if(reference_position == STEP_MOTOR_START_RESET_POSITION)
        reference_position = start_position;

    int end_position = 0;
    ret = getEndPositionInPulse(pulse_from_reference, resolution_level, direction_from_reference, end_position, reference_position);
    if (ret < 0)
        return ret;

    ret = setParameterInPositionInPulse(end_position, max_frequency_, resolution_level, STEP_MOTOR_RAMP_PULSE_0, STEP_MOTOR_HIRES_MODE_OFF);
    if (ret < 0)
        return ret;

    run();

    waitMotorToStop(); 

    return ret;
}

int StepMotorUser::resetAndSetParameterInDegree(int reset_dir, double angle_from_reference, int direction_from_reference, double reference_position) {
    // 复位，回弹，到指定位置，都使用4细分
    const int resolution_level = STEP_MOTOR_MINISTEP_4;
    const int resolution = 4;

    double start_position = 0;
    int ret = resetInDegree(reset_dir, start_position);
    if (ret < 0)
        return ret;

    if(reference_position == STEP_MOTOR_START_RESET_POSITION)
        reference_position = start_position;

    double end_position = 0;
    ret = getEndPositionInDegree(angle_from_reference, resolution_level, direction_from_reference, end_position, reference_position);
    if (ret < 0)
        return ret;

    ret = setParameterInPositionInDegree(end_position, max_frequency_, resolution_level, STEP_MOTOR_RAMP_PULSE_0, STEP_MOTOR_HIRES_MODE_OFF);
    if (ret < 0)
        return ret;

    return ret;
}

int StepMotorUser::resetAndSetParameterInPulse(int reset_dir, int pulse_from_reference, int direction_from_reference, int reference_position) {
    // 复位，回弹，到指定位置，都使用4细分
    const int resolution_level = STEP_MOTOR_MINISTEP_4;
    const int resolution = 4;

    int start_position = 0;
    int ret = resetInPulse(reset_dir, start_position);
    if (ret < 0)
        return ret;

    if(reference_position == STEP_MOTOR_START_RESET_POSITION)
        reference_position = start_position;

    int end_position = 0;
    ret = getEndPositionInPulse(pulse_from_reference, resolution_level, direction_from_reference, end_position, reference_position);
    if (ret < 0)
        return ret;

    ret = setParameterInPositionInPulse(end_position, max_frequency_, resolution_level, STEP_MOTOR_RAMP_PULSE_0, STEP_MOTOR_HIRES_MODE_OFF);
    if (ret < 0)
        return ret;

    return ret;
}

int StepMotorUser::runBacklash() {
    log_i("RUN BACKLASH!");

    int pulse = 0;
    int ret = angle2Pulse(backlash_, resolution_, pulse);
    if (ret < 0)
        return ret;

    pulse_ = pulse;
    direction_ = !direction_;
    
    run();

    return waitMotorToStop();
}

// 执行电机参数检查
int StepMotorUser::setParameterInDegree(double angle, double speed, int resolution_level, int speed_ramping, bool direction, bool high_resolution_mode) {
    if (STEP_MOTOR_LIMITED == getLimitState())
        waitMotorToStop(2);

    if (STEP_MOTOR_RUNNING == getMotionState()) 
        stop();
    
    double resolution = 0;
    int ret = resolutionLevel2Resolution(resolution_level, resolution);
    if (ret < 0)
        return ret;

    int frequency = 0;
    ret = speed2Frequency(speed, resolution, high_resolution_mode, frequency);
    if (ret < 0)
        return ret;

    // // 反向时处理回程，先多走，再回走
    // if (direction != direction_) {
    //     log_i("BACKLASH EVENT HAPPEN!");
    //     backlash_event_ = 1;

    //     ret = checkAngle(angle);
    //     if (ret < 0)
    //         return ret;

    //     angle += backlash_;
    // }

    int pulse = 0;
    ret = angle2Pulse(angle, resolution, pulse);
    if (ret < 0)
        return ret;

    ret = checkSpeedRamping(frequency, speed_ramping);
    if (ret < 0)
        return ret;

    double end_position = 0;
    if(getEndPositionInDegree(angle, resolution_level, direction, end_position) >= 0)
        ret = checkPositionInDegree(end_position);
    
    angle_ = angle;
    pulse_ = pulse;
    speed_ = speed;
    frequency_ = frequency;
    resolution_level_ = resolution_level;
    resolution_ = resolution;
    speed_ramping_ = speed_ramping;
    direction_ = direction;
    infinity_mode_ = STEP_MOTOR_INFINITY_MODE_OFF;
    high_resolution_mode_ = high_resolution_mode;

    calculateRunTime();
    
    printAllParameter();

    return ret;
}

// 使用缓起时间
int StepMotorUser::setParameterInDegree(double angle, double speed, int resolution_level, double speed_ramping_time, bool direction, bool high_resolution_mode) {
    if (STEP_MOTOR_LIMITED == getLimitState())
        waitMotorToStop(2);

    if (STEP_MOTOR_RUNNING == getMotionState()) 
        stop();
    
    double resolution = 0;
    int ret = resolutionLevel2Resolution(resolution_level, resolution);
    if (ret < 0)
        return ret;

    int frequency = 0;
    ret = speed2Frequency(speed, resolution, high_resolution_mode, frequency);
    if (ret < 0)
        return ret;

    // // 反向时处理回程，先多走，再回走
    // if (direction != direction_) {
    //     log_i("BACKLASH EVENT HAPPEN!");
    //     backlash_event_ = 1;

    //     ret = checkAngle(angle);
    //     if (ret < 0)
    //         return ret;

    //     angle += backlash_;
    // }

    int pulse = 0;
    ret = angle2Pulse(angle, resolution, pulse);
    if (ret < 0)
        return ret;

    int speed_ramping = int(speed_ramping_time * (double)frequency / 2.0);
    ret = checkSpeedRamping(frequency, speed_ramping);
    if (ret < 0)
        return ret;

    double end_position = 0;
    if(getEndPositionInDegree(angle, resolution_level, direction, end_position) >= 0)
        ret = checkPositionInDegree(end_position);
    
    angle_ = angle;
    pulse_ = pulse;
    speed_ = speed;
    frequency_ = frequency;
    resolution_level_ = resolution_level;
    resolution_ = resolution;
    speed_ramping_ = speed_ramping;
    direction_ = direction;
    infinity_mode_ = STEP_MOTOR_INFINITY_MODE_OFF;
    high_resolution_mode_ = high_resolution_mode;

    calculateRunTime();
    
    printAllParameter();

    return ret;
}

int StepMotorUser::setParameterInPositionInDegree(double end_position, double speed, int resolution_level, int speed_ramping, bool high_resolution_mode) {
    int ret = checkPositionInDegree(end_position);
    if (ret < 0)
        return ret;
    
    double resolution = 0;
    ret = resolutionLevel2Resolution(resolution_level, resolution);
    if (ret < 0)
        return ret;
    
    int end_position_in_pulse = 0;
    ret = angle2Pulse(end_position, resolution, end_position_in_pulse);
    if (ret < 0)
        return ret;

    int frequency = 0;
    ret = speed2Frequency(speed, resolution, high_resolution_mode, frequency);
    if (ret < 0)
        return ret;

    return setParameterInPositionInPulse(end_position_in_pulse, frequency, resolution_level, speed_ramping, high_resolution_mode);
}

int StepMotorUser::setParameterInPositionInPulse(int end_position, int frequency, int resolution_level, int speed_ramping, bool high_resolution_mode) {
    if (STEP_MOTOR_LIMITED == getLimitState())
        waitMotorToStop(2);

    if (STEP_MOTOR_RUNNING == getMotionState()) 
        stop();

    double resolution = 0;
    int ret = resolutionLevel2Resolution(resolution_level, resolution);
    if (ret < 0) 
        return ret;

    ret = checkPositionInPulse(end_position, resolution);
    if (ret < 0)
        return ret;

    int now_position = 0;
    ret = getAbsolutePositionInPulse(resolution_level, now_position);   
    if (ret < 0)
        return ret;

    int pulse_to_end = end_position - now_position;

    if (pulse_to_end > 0)
        return setParameterInPulse(pulse_to_end, frequency, resolution_level, speed_ramping, !reset_dir_, high_resolution_mode);
    else
        return setParameterInPulse(std::abs(pulse_to_end), frequency, resolution_level, speed_ramping, reset_dir_, high_resolution_mode);
}

int StepMotorUser::setParameterInPulse(int pulse, int frequency, int resolution_level, int speed_ramping, bool direction, bool high_resolution_mode) {
    if (STEP_MOTOR_LIMITED == getLimitState())
        waitMotorToStop(2);

    if (STEP_MOTOR_RUNNING == getMotionState()) 
        stop();
    
    double resolution = 0;
    int ret = resolutionLevel2Resolution(resolution_level, resolution);
    if (ret < 0) 
        return ret;

    double speed = 0;
    ret = frequency2Speed(frequency, resolution, high_resolution_mode, speed);
    if (ret < 0) 
        return ret;

    // // 反向时处理回程，先多走，再回走
    // if (direction != direction_) {
    //     log_i("BACKLASH EVENT HAPPEN!");
    //     backlash_event_ = 1;

    //     int pulse_backlash = 0;
    //     ret = angle2Pulse(backlash_, resolution, pulse_backlash);
    //     if (ret < 0)
    //         return ret;

    //     pulse += pulse_backlash;
    // }

    double angle = 0;
    ret = pulse2Angle(pulse, resolution, angle);
    if (ret < 0) 
        return ret;

    ret = checkSpeedRamping(frequency, speed_ramping);
    if (ret < 0) 
        return ret;

    double end_position = 0;
    if(getEndPositionInDegree(angle, resolution_level, direction, end_position) >= 0)
        ret = checkPositionInDegree(end_position);

    angle_ = angle;
    pulse_ = pulse;
    speed_ = speed;
    frequency_ = frequency;
    resolution_level_ = resolution_level;
    resolution_ = resolution;
    speed_ramping_ = speed_ramping;
    direction_ = direction;
    infinity_mode_ = STEP_MOTOR_INFINITY_MODE_OFF;
    high_resolution_mode_ = high_resolution_mode;

    calculateRunTime();
    
    printAllParameter();

    return ret;
}

int StepMotorUser::setParameterInDegree(double speed, int resolution_level, int speed_ramping, bool direction, bool high_resolution_mode) {
    if (STEP_MOTOR_LIMITED == getLimitState())
        waitMotorToStop(2);

    if (STEP_MOTOR_RUNNING == getMotionState()) {
        // log_e("INVALID COMMAND: StepMotor %d is running, speed: %f", motor_id_, speed_);
        return -CODE_STEP_MOTOR_IS_RUNNING;
    }
        
    double resolution = 0;
    int ret = resolutionLevel2Resolution(resolution_level, resolution);
    if (ret < 0)
        return ret;

    int frequency = 0;
    ret = speed2Frequency(speed, resolution, high_resolution_mode, frequency);
    if (ret < 0)
        return ret;

    ret = checkSpeedRamping(frequency, speed_ramping);
    if (ret < 0)
        return ret;
    
    angle_ = 0;
    pulse_ = 0;
    speed_ = speed;
    frequency_ = frequency;
    resolution_level_ = resolution_level;
    resolution_ = resolution;
    speed_ramping_ = speed_ramping;
    direction_ = direction;
    infinity_mode_ = STEP_MOTOR_INFINITY_MODE_ON;
    high_resolution_mode_ = high_resolution_mode;

    calculateRunTime();
    
    printAllParameter();

    return ret;
}

int StepMotorUser::setParameterInDegree(double speed, int resolution_level, double speed_ramping_time, bool direction, bool high_resolution_mode) {
    if (STEP_MOTOR_LIMITED == getLimitState())
        waitMotorToStop(2);

    if (STEP_MOTOR_RUNNING == getMotionState()) {
        // log_e("INVALID COMMAND: StepMotor %d is running, speed: %f", motor_id_, speed_);
        return -CODE_STEP_MOTOR_IS_RUNNING;
    }
        
    double resolution = 0;
    int ret = resolutionLevel2Resolution(resolution_level, resolution);
    if (ret < 0)
        return ret;

    int frequency = 0;
    ret = speed2Frequency(speed, resolution, high_resolution_mode, frequency);
    if (ret < 0)
        return ret;

    int speed_ramping = int(speed_ramping_time * (double)frequency / 2.0);
    ret = checkSpeedRamping(frequency, speed_ramping);
    if (ret < 0)
        return ret;
    
    angle_ = 0;
    pulse_ = 0;
    speed_ = speed;
    frequency_ = frequency;
    resolution_level_ = resolution_level;
    resolution_ = resolution;
    speed_ramping_ = speed_ramping;
    direction_ = direction;
    infinity_mode_ = STEP_MOTOR_INFINITY_MODE_ON;
    high_resolution_mode_ = high_resolution_mode;

    calculateRunTime();
    
    printAllParameter();

    return ret;
}

int StepMotorUser::setParameterInPulse(int frequency, int resolution_level, int speed_ramping, bool direction, bool high_resolution_mode) {
    if (STEP_MOTOR_LIMITED == getLimitState())
        waitMotorToStop(2);

    if (STEP_MOTOR_RUNNING == getMotionState()) {
        // log_e("INVALID COMMAND: StepMotor %d is running", motor_id_);
        return -CODE_STEP_MOTOR_IS_RUNNING;
    }
    
    double resolution = 0;
    int ret = resolutionLevel2Resolution(resolution_level, resolution);
    if (ret < 0)
        return ret;

    double speed = 0;
    ret = frequency2Speed(frequency, resolution, high_resolution_mode, speed);
    if (ret < 0)
        return ret;

    ret = checkSpeedRamping(frequency, speed_ramping);
    if (ret < 0)
        return ret;
    
    angle_ = 0;
    pulse_ = 0;
    speed_ = speed;
    frequency_ = frequency;
    resolution_level_ = resolution_level;
    resolution_ = resolution;
    speed_ramping_ = speed_ramping;
    direction_ = direction;
    infinity_mode_ = STEP_MOTOR_INFINITY_MODE_ON;
    high_resolution_mode_ = high_resolution_mode;

    calculateRunTime();
    
    printAllParameter();

    return ret;
}

int StepMotorUser::run() {
    if (STEP_MOTOR_LIMITED == getLimitState())
        waitMotorToStop(2);

    if (STEP_MOTOR_RUNNING == getMotionState()) 
        stop();

    int ret = checkResolutionLevel(resolution_level_);
    if (ret < 0)
        return ret;

    ret = checkFrequency(frequency_, resolution_, high_resolution_mode_);
    if (ret < 0)
        return ret;
    else if (ret == CODE_STEP_MOTOR_LIMIT_SPEED_TO_MIN)// 速度太小，不run
        return ret;

    ret = checkSpeedRamping(frequency_, speed_ramping_);
    if (ret < 0)
        return ret;

    ret = checkPulse(pulse_);
    if (ret < 0)
        return ret;

    calculateRunTime();

    int mode = 0;
    int frequency = frequency_;

    if (high_resolution_mode_) {
        frequency = frequency_ * 100;
        if (infinity_mode_) 
            mode = STEP_MOTOR_HIRES_INFINITY_MODE;
        else 
            mode = STEP_MOTOR_HIRES_PULSE_MODE;
    } else {
        if (infinity_mode_) 
            mode = STEP_MOTOR_NORMAL_INFINITY_MODE;
        else 
            mode = STEP_MOTOR_NORMAL_PULSE_MODE;
    }

    driver.run(mode, resolution_level_, direction_, frequency, pulse_, speed_ramping_);

    // if (backlash_event_) {
    //     backlash_event_ = 0;
    //     waitMotorToStop();
    //     ret = runBacklash();
    // }

    return ret;
}

int StepMotorUser::stop() {
    // driver.stop(1, 0, 0);
    driver.stop(0, 0, speed_ramping_);
    usleep(10 * 1000);

    double decelerate_time = 2.0 * (double)speed_ramping_ / (double)frequency_;

    return waitMotorToStop(decelerate_time);         
}

int StepMotorUser::waitMotorToStop() {                                  
    int ret = waitMotorToStop(run_time_);

    // 等待电机回弹
    if (waitMotorToLimit(0.02) == 0) {
        waitMotorToRun(0.2);
        waitMotorToStop(2);

        log_e("HIT LIMIT: StepMotor %d run %f deg hit limit", motor_id_, angle_);
        ret = -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED;
    }

    return ret;
}

int StepMotorUser::resetInDegree(int direction, double& start_position) {
    // 复位固定4细分
    const double resolution = 4;
    int start_position_in_pulse = 0;
    
    int ret = resetInPulse(direction, start_position_in_pulse);
    if(ret < 0)
        return ret;

    pulse2Angle(start_position_in_pulse, resolution, start_position);

    return ret;
}

int StepMotorUser::resetInPulse(int direction, int& start_position) {
    if (STEP_MOTOR_RUNNING == getMotionState()) 
        stop();

    driver.reset(direction);

    int reset_position = 0;
    int ret = waitMotorToReset(reset_position);
    if (ret < 0) {
        // 如果还不停，可能限位坏了
        if (STEP_MOTOR_RUNNING == getMotionState()) {
            stop();
            return ret;
        }
        
        // // 以下内容不注释，复位时发停止指令，停了又会动起来
        // // 丢了复位完成返回，再复位一次
        // driver.reset(direction);

        // int trash = 0;
        // ret = waitMotorToReset(trash);
        
        // if (ret < 0) {
        //     // 如果还不停，可能限位坏了
        //     if (STEP_MOTOR_RUNNING == getMotionState())
        //         stop();

        //     return ret;
        // }
    }

    reset_dir_ = direction;
    direction_ = direction;
    start_position = reset_position;

    return ret;
}

int StepMotorUser::waitMotorToReset(int& reset_position) {            
    waitMotorToStop(reset_time_s_);    
    reset_position = driver.getMotionPulse();         

    // 等待电机回弹
    waitMotorToRun(0.2);
    waitMotorToStop(2);

    int distance = driver.getResetState();
    if (distance < 0) {
        // log_e("RETURN OVERTIME: StepMotor %d reset", motor_id_);
        return -CODE_STEP_MOTOR_OVERTIME_GET_RESET_RETURN;
    }
    
    reset_position = distance;

    return 0;
}

int StepMotorUser::waitMotorToStop(double wait_time_s) {                    
    // log_i("wait motor %d to stop", motor_id_);

    usleep(10 * 1000);  // 等待电机执行指令
    if (STEP_MOTOR_STOPPED == getMotionState())
        return 0;

    // 最短刷新间隔10ms
    int wait_time_tms = std::ceil(wait_time_s * 100);                                  
    for(int i = 0; i < wait_time_tms; i++) {
        usleep(10 * 1000);
        if (STEP_MOTOR_STOPPED == driver.getMotionState())
            return 0;
    }

    // log_e("WAIT TO STOP OVERTIME: StepMotor %d stop", motor_id_);

    return -CODE_STEP_MOTOR_OVERTIME_WAIT_TO_STOP;
}

int StepMotorUser::waitMotorToRun(double wait_time_s) {                    
    // log_i("wait motor %d to run", motor_id_);

    usleep(10 * 1000);  // 等待电机执行指令
    if (STEP_MOTOR_RUNNING == getMotionState())
        return 0;

    // 最短刷新间隔10ms
    int wait_time_tms = std::ceil(wait_time_s * 100);                                  
    for(int i = 0; i < wait_time_tms; i++) {
        usleep(10 * 1000);
        if (STEP_MOTOR_RUNNING == driver.getMotionState())
            return 0;
    }

    // log_e("WAIT TO RUN OVERTIME: StepMotor %d run", motor_id_);

    return -CODE_STEP_MOTOR_OVERTIME_WAIT_TO_RUN;
}

int StepMotorUser::waitMotorToLimit(double wait_time_s) {                    
    // log_i("wait motor %d to run", motor_id_);

    // 最短刷新间隔10ms
    int wait_time_tms = std::ceil(wait_time_s * 100);                                  
    for(int i = 0; i < wait_time_tms; i++) {
        usleep(10 * 1000);
        if (STEP_MOTOR_LIMITED == getLimitState())
            return 0;
    }

    // log_e("WAIT TO LIMIT OVERTIME: StepMotor %d unlimited", motor_id_);

    return -CODE_STEP_MOTOR_OVERTIME_GET_LIMIT_RETURN;
}

int StepMotorUser::changeSpeed(double speed, bool print_log) {
    if (STEP_MOTOR_STOPPED == driver.getMotionState()) {
        // log_e("INVALID COMMAND: StepMotor %d stopped", motor_id_);
        return -CODE_STEP_MOTOR_IS_STOPPED;
    }

    int speed_trend = speed > speed_ ? STEP_MOTOR_SPEED_UP : STEP_MOTOR_SLOW_DOWN;
    int frequency = 0;
    int ret = speed2Frequency(speed, resolution_, high_resolution_mode_, frequency);
    if (ret < 0) 
        return ret;

    int speed_ramping = int((double)frequency / (double)frequency_ * (double)speed_ramping_);
    if (checkSpeedRamping(frequency, speed_ramping) < 0)
        speed_ramping_ = STEP_MOTOR_RAMP_PULSE_1000;
    else
        speed_ramping_ = speed_ramping;

    if (ret == CODE_STEP_MOTOR_LIMIT_SPEED_TO_MIN) {   // 速度太小，直接停
        // log_i("INFORMED: set speed %f is too small, StepMotor %d change speed %f to 0.0", speed, motor_id_, speed_);
        stop();
        return ret;
    }

    if (print_log)
        log_i("INFORMED: StepMotor %d change speed %f to %f, frequency %d to %d", motor_id_, speed_, speed, frequency_, frequency);

    speed_ = speed;
    frequency_ = frequency;

    calculateRunTime();

    driver.setFrequency(speed_trend, frequency_, 0);

    return ret;
}

int StepMotorUser::changeFrequency(int frequency) {
    if (STEP_MOTOR_STOPPED == driver.getMotionState()) {
        // log_e("INVALID COMMAND: StepMotor %d stopped", motor_id_);
        return -CODE_STEP_MOTOR_IS_STOPPED;
    }

    int speed_trend = frequency > frequency_ ? STEP_MOTOR_SPEED_UP : STEP_MOTOR_SLOW_DOWN;
    double speed = 0;
    int ret = frequency2Speed(frequency, resolution_, high_resolution_mode_, speed);
    if (ret < 0) 
        return ret;

    int speed_ramping = int((double)frequency / (double)frequency_ * (double)speed_ramping_);
    if (checkSpeedRamping(frequency, speed_ramping) < 0)
        speed_ramping_ = STEP_MOTOR_RAMP_PULSE_1000;
    else
        speed_ramping_ = speed_ramping;

    if (ret == CODE_STEP_MOTOR_LIMIT_SPEED_TO_MIN) {   // 速度太小，直接停
        // log_i("INFORMED: frequency %d is too small, StepMotor %d change frequency %d to 0.0", frequency, motor_id_, frequency_);
        stop();
        return ret;
    }
        
    // log_i("INFORMED: StepMotor %d change frequency %d to %d", motor_id_, frequency_, frequency);

    speed_ = speed;
    frequency_ = frequency;

    calculateRunTime();

    driver.setFrequency(speed_trend, frequency_, 0);

    return ret;
}

int StepMotorUser::changeDirection(bool direction, bool print_log) {
    if (STEP_MOTOR_STOPPED == driver.getMotionState()) {
        // log_e("INVALID COMMAND: StepMotor %d stopped", motor_id_);
        return -CODE_STEP_MOTOR_IS_STOPPED;
    }

    if (print_log)
        log_i("INFORMED: StepMotor %d change direction %d to %d", motor_id_, direction_, direction);

    direction_ = direction;
    driver.setDirection(direction_);

    return 0;
}

int StepMotorUser::getEndPositionInPulse(int pulse, int resolution_level, int direction, int& end_position, int reference_position) {
    int ret = checkPulse(pulse);
    if (ret < 0) 
        return ret;

    if (reference_position == STEP_MOTOR_NOW_POSITION) {
        ret = getAbsolutePositionInPulse(resolution_level, reference_position);
        if (ret < 0) 
            return ret;
    }

    double resolution = 0;
    ret = resolutionLevel2Resolution(resolution_level, resolution);
    if (ret < 0)
        return ret;

    ret = checkPositionInPulse(reference_position, resolution);
    if (ret < 0) 
        return ret;
    
    end_position = direction == reset_dir_ ? reference_position - pulse : reference_position + pulse;

    return ret;
}

// 返回值仅用来判断是否获取到了终点位置，不与限位预警耦合
int StepMotorUser::getEndPositionInDegree(double angle, int resolution_level, int direction, double& end_position, double reference_position) {
    int ret = checkAngle(angle);
    if (ret < 0) 
        return ret;

    if (reference_position == STEP_MOTOR_NOW_POSITION) {
        ret = getAbsolutePositionInDegree(resolution_level, reference_position);
        if (ret < 0) 
            return ret;
    }

    ret = checkPositionInDegree(reference_position);
    if (ret < 0) 
        return ret;
    
    end_position = direction == reset_dir_ ? reference_position - angle : reference_position + angle;

    return ret;
}

bool StepMotorUser::getMotionState() {
    driver.checkMotionState();
    usleep(10 * 1000);

    return driver.getMotionState();
}

bool StepMotorUser::getLimitState() {
    return driver.getLimitState();
}

bool StepMotorUser::getLimitState(int direciton) {
    return driver.getLimitState(direciton);
}

int StepMotorUser::getAbsolutePositionInDegree(int resolution_level, double& absolute_position_angle) {
    int absolute_position_pulse = 0;
    int ret = getAbsolutePositionInPulse(resolution_level, absolute_position_pulse);
    if(ret < 0)
        return ret;

    double resolution = 0;
    ret = resolutionLevel2Resolution(resolution_level, resolution);
    if (ret < 0)
        return ret;

    return pulse2Angle(absolute_position_pulse, resolution, absolute_position_angle);
}

int StepMotorUser::getAbsolutePositionInPulse(int resolution_level, int& absolute_position) {
    int wait_time_tms = 5;

    driver.readAbsolutePosition(resolution_level);

    for(int i = 0; i < wait_time_tms; i++) {
        usleep(10 * 1000);
        int l_absolute_position = driver.getAbsolutePosition();
        if (l_absolute_position >= 0) {
            absolute_position = l_absolute_position;
            return 0;
        } else if (l_absolute_position == -2) {
            // log_e("COMMAND ERROR: StepMotor %d need reset", motor_id_);
            return -CODE_STEP_MOTOR_NEED_RESET;
        }
    }

    // log_e("RETURN OVERTIME: StepMotor %d get absolute position", motor_id_);

    return -CODE_STEP_MOTOR_OVERTIME_GET_ABSOLUTE_POSITION_RETURN;
}

int StepMotorUser::getRelativePosition(int& relative_position) {
    int wait_time_tms = 5;

    driver.readRelativePosition();

    for(int i = 0; i < wait_time_tms; i++) {
        usleep(10 * 1000);
        int l_relative_position = driver.getRelativePosition();
        if (l_relative_position >= 0) {
            relative_position = l_relative_position;
            return 0;
        }
    }

    // log_e("RETURN OVERTIME: StepMotor %d get relative position", motor_id_);

    return -CODE_STEP_MOTOR_OVERTIME_GET_RELATIVE_POSITION_RETURN;
}

int StepMotorUser::checkPositionInPulse(int position, double resolution) {
    if (position < 0) {
        // log_e("INVALID PARAMETER: StepMotor %d position = %d, resolution = %f", motor_id_, position, resolution);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_POSITION;
    }
    
    // TODO: 度与脉冲来回转换，存在误差
    double angle = 0;
    int ret = pulse2Angle(position, resolution, angle);
    if (ret < 0)
        return ret;
    
    if (angle < min_soft_limit_ || angle > max_soft_limit_) {
        // log_e("INVALID PARAMETER: StepMotor %d position = %f deg limit warning", motor_id_, angle);
        return CODE_STEP_MOTOR_LIMIT_POSITION_WARNING;
    }

    return 0;
}

int StepMotorUser::checkPositionInDegree(double position) {
    if (position < 0) {
        // log_e("INVALID PARAMETER: StepMotor %d position = %f", motor_id_, position);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_POSITION;
    }
    
    if (position < min_soft_limit_ || position > max_soft_limit_) {
        // log_e("INVALID PARAMETER: StepMotor %d position = %f deg limit warning", motor_id_, position);
        return CODE_STEP_MOTOR_LIMIT_POSITION_WARNING;
    }

    return 0;
}

int StepMotorUser::resolutionLevel2Resolution(int resolution_level, double& resolution) {
    int ret = checkResolutionLevel(resolution_level);

    if(ret >= 0)
        resolution = std::pow(2, (8 - resolution_level));

    return ret;
}

int StepMotorUser::speed2Frequency(double speed, double resolution, bool high_resolution_mode, int& frequency) {
    double p2a_coefficient = step_angle_ / resolution / gear_ratio_;
    int l_frequency = int(speed / p2a_coefficient);

    int ret = checkFrequency(l_frequency, resolution, high_resolution_mode);
    if(ret >= 0)
        frequency = l_frequency;

    return ret;
}

int StepMotorUser::frequency2Speed(int& frequency, double resolution, bool high_resolution_mode, double& speed) {
    int ret = checkFrequency(frequency, resolution, high_resolution_mode);

    double p2a_coefficient = step_angle_ / resolution / gear_ratio_;

    if(ret >= 0)
        speed = double(frequency) * p2a_coefficient;

    return ret;
}

int StepMotorUser::angle2Pulse(double angle, double resolution, int& pulse) {
    double p2a_coefficient = step_angle_ / resolution / gear_ratio_;
    int l_pulse = std::round(angle / p2a_coefficient);

    int ret = checkPulse(l_pulse);
    if (ret >= 0)
        pulse = l_pulse;

    return ret;
}

int StepMotorUser::pulse2Angle(int pulse, double resolution, double& angle) {
    int ret = checkPulse(pulse);

    double p2a_coefficient = step_angle_ / resolution / gear_ratio_;

    if(ret >= 0)
        angle = double(pulse) * p2a_coefficient;

    return ret;
}

int StepMotorUser::checkFrequency(int& frequency, double resolution, bool high_resolution_mode) {
    int max_frequency = resolution * 1000 < max_frequency_ ? resolution * 1000 : max_frequency_;
    max_frequency = high_resolution_mode ? hires_max_frequency_ : max_frequency;

    if (frequency >= 0) {
        if (frequency < min_frequency_) {
            // log_e("INVALID PARAMETER: StepMotor %d frequency = %d is too small", motor_id_, frequency);

            frequency = 1;// 避免0频率，求运动时间时除0，低于最低速度时，不会发送运动指令，直接停止

            return CODE_STEP_MOTOR_LIMIT_SPEED_TO_MIN;
        } else if (frequency > max_frequency) {
            // log_e("INVALID PARAMETER: StepMotor %d frequency = %d is too large, limit to %d", motor_id_, frequency, max_frequency);

            frequency = max_frequency;

            return CODE_STEP_MOTOR_LIMIT_SPEED_TO_MAX;
        }
        return 0;
    } else {
        // log_e("INVALID PARAMETER: StepMotor %d frequency = %d < 0", motor_id_, frequency);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_SPEED;
    }
}

int StepMotorUser::checkPulse(int pulse) {
    if (pulse >= 0) {
        return 0;
    } else {
        // log_e("INVALID PARAMETER: StepMotor %d pulse = %d < 0", motor_id_, pulse);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_ANGLE;
    }
}

int StepMotorUser::checkAngle(double angle) {
    if (angle >= 0) {
        return 0;
    } else {
        // log_e("INVALID PARAMETER: StepMotor %d angle = %f < 0", motor_id_, angle);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_ANGLE;
    }
}

int StepMotorUser::checkSpeedRamping(int frequency, int& speed_ramping) {
    if (frequency >= 320 && speed_ramping >= STEP_MOTOR_RAMP_PULSE_0 && speed_ramping <= STEP_MOTOR_RAMP_PULSE_1000) {
        return 0;
    } else  {
        if (frequency < 320) {
            // log_e("WARNING: StepMotor %d frequency %d < 320, invalid speed_ramping %d limit to %d", motor_id_, frequency, speed_ramping, STEP_MOTOR_RAMP_PULSE_0);
            speed_ramping = STEP_MOTOR_RAMP_PULSE_0;
            return 0;
        }

        if (speed_ramping > STEP_MOTOR_RAMP_PULSE_1000) {
            // log_e("WARNING: StepMotor %d speed_ramping %d is too big, limit to %d", motor_id_, speed_ramping, STEP_MOTOR_RAMP_PULSE_1000);
            speed_ramping = STEP_MOTOR_RAMP_PULSE_1000;
            return 0;
        }

        // log_e("INVALID PARAMETER: StepMotor %d speed_ramping = %d", motor_id_, speed_ramping);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_SPEED_RAMPING;
    }
}

int StepMotorUser::checkResolutionLevel(int resolution_level) {
    if (resolution_level >= STEP_MOTOR_MINISTEP_256  && resolution_level <= STEP_MOTOR_MINISTEP_1) {
        return 0;
    } else {
        // log_e("INVALID PARAMETER: StepMotor %d resolution_level = %d", motor_id_, resolution_level);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_RESOLUTION;
    }
}

void StepMotorUser::calculateRunTime() {
    double const_speed_run_time = pulse_ - 2 * speed_ramping_ <= 0 ? 0 : (double)(pulse_ - 2 * speed_ramping_) / (double)frequency_;
    run_time_ = const_speed_run_time + 2.0 * 2.0 * (double)speed_ramping_ / (double)frequency_;
}

void StepMotorUser::printAllParameter() const {
    log_i("id:%d, angle:%f, speed:%f, pulse:%d, freq:%d, res:%f, ramp:%d, dir:%d, infnt:%d, hires:%d, time:%f", 
           motor_id_, angle_, speed_, pulse_, frequency_, resolution_, speed_ramping_, direction_, infinity_mode_, high_resolution_mode_, run_time_);
}